#ifndef __CHASSIS_H__
#define __CHASSIS_H__

#include "pid.h"
#include "kinematics.h"
#include "BSP_Motor.h"
#include "BSP_Usart.h"

#define Chassis_Kp						4.3
#define Chassis_Ki						0.35
#define Chassis_Kd						0.05

#define set_chassis_speed(motor1_speed,motor2_speed,motor3_speed,motor4_speed) \
        do{                                                                    \
					chassis_motor1.vpid.target_speed = motor1_speed;	     \
	        chassis_motor2.vpid.target_speed = motor2_speed;      \
	        chassis_motor3.vpid.target_speed = motor3_speed;      \
	        chassis_motor4.vpid.target_speed = motor4_speed;      \
				}while(0)                                                              \

extern MOTOR_t chassis_motor1,chassis_motor2,chassis_motor3,chassis_motor4;
void CAN1_Chassis_Msg(uint8_t *mdata);
void Chassis_Init(void);
void Chassis_move_Task(void);
	
#endif

